
#include "smaAPI.h"
#include "LabviewBridge.h"


extern Labview_socket_Data Lsocket;

extern int smaInitLibrary(void);
extern int smaDeinitLibrary(void);
extern int smaConnect(enum SmaProcessor Processor, const char *Address, unsigned short Port);
extern int smaDisconnect(enum SmaProcessor Processor);


int main(void)
{
	int ret;

	// ***** init SMA API*****
	ret = smaInitLibrary();
	if(ret < 0)
	{
		printf("Error at smaInitLibrary()! exit\n");
		return -1;
	}
	ret = Labview_socket_Init();
	if(ret < 0)
	{
		printf("Error at Labview_socket_Init()! exit\n");
		return -1;
	}

	connectN: 
	ret = smaConnect(MCP, "192.168.1.22", 12345);
	if(ret < 0)
	{
		printf("Error at smaConnect()\n");
		goto connectN;
	}
	ret = Labview_socket_Start();
	if(ret < 0)
	{
		printf("Error at Labview Create Scoket _Init() \n");
		goto connectN;
	}

	// *** routing control ***
	while(1)
	{
		ret = smaTeleOpArmJoint(Shoulder, MotStpMtn, 2.1234);
		if(ret < 0)
		{
			printf("Error at smaTeleOpArmJoint()\n");
			break;
		}
		Sleep(1000);
		printf("|");
	}

	// *** close
	ret = smaDisconnect(MCP);
	if(ret < 0)
	{
		printf("Error at smaDisconnect()\n");
		return -1;
	}
    ret = Labview_socket_Stop();
	if(ret < 0)
	{
		printf("Error at smaDisconnect()\n");
		return -1;
	}

	goto connectN;

	ret = smaDeinitLibrary();
	if(ret < 0)
	{
		printf("Error at smaDeinitLibrary()\n");
		return -1;
	}

	ret = Labview_socket_De_init();
		if(ret < 0)
	{
		printf("Error at Labview_socket_De_init()\n");
		return -1;
	}

	return 0;
}

int callback_Labview_Callback(p_Labview_socket_Data sData)
{
	int i;
	for(i=0;i<sData->receive_len;i++)
	{
		sData->reply_buff[i] = sData->receive_buff[i];
	}
	sData->reply_len = sData->receive_len;
	return 0;
}